#include "Code0_CONF.h"		
#include "Code1_MOTOR.h"		
#include "Code4_MOTION.h"				 
#include "Code5_DBL.h"
#include "Code6_HEAD.h"
#include "Data0_AIRSA_SETTING.h"
		
MotorRXStruct MotorRX[BODYPART_NUM];
MotorDataStruct MotorData;
MotorWriteAddressStruct MotorAddress;	 
MotortTorqueStruct MotorTorque;			 
MotorWriteAddressStruct MotorTorqueAddress;	 
MotorScheduleStruct MotorSchedule;			
HeadScheduleStruct HeadSchedule;

HeadMotionStruct Head;

Bool MotorBeWrited = FALSE;
Bool HeadBeWrited = FALSE;
Bool MotorWritedFinish[BODYPART_NUM] = {TRUE, TRUE, TRUE};

int MotorReadID[BODYPART_NUM] = {0};	
int Steptime_WriteMotor = WRITE_MOTOR_STEPTIME;				
int Steptime_WriteHead = WRITE_HEAD_STEPTIME;	

extern MotionStruct Motion;		
extern unsigned char Zigbee_Ready2Disable;
extern Bool Zigbee_Disabled;
																																							 
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : Motor_Init
* Description    : Execute the initialization of Motor
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void Motor_Init(void)
{
	MotorRX_Init();
	MotorAddress_Init();	
	MotorData_Init();
	MotorSchedule_Init();				
	HeadSchedule_Init(); 
	DIR485_Receive(MOTOR_UB_PORT);	 
	DIR485_Receive(MOTOR_RL_PORT);
	DIR485_Receive(MOTOR_LL_PORT);
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////		 
/*******************************************************************************
* Function Name  : MotorRX_Init
* Description    : Initial MotorRX
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void MotorRX_Init(void)
{
	int i, j;
	
	for(i=0; i < BODYPART_NUM; i++)
	{
		for(j=0; j < MOTOR_RX_BUFF_SIZE; j++)
		{
			MotorRX[i].Buff[j] = 0;
		}
		MotorRX[i].NowIndex = 0;
		MotorRX[i].EndIndex = 0;
	}
}
/*******************************************************************************
* Function Name  : MotorAddress_Init
* Description    : Assign the pointer of MotorData.Write to the member MotorAddress
* Input          : None
* Output         : None
* Return         : None
* Attention		 : Unchangable after initialing
*******************************************************************************/
void MotorAddress_Init(void)
{
	MotorAddress.PacketHead[UPPER_BODY] = &(MotorData.PacketHead_UB[0]);
	MotorAddress.PacketHead[RIGHT_LEG] = &(MotorData.PacketHead_RL[0]);	
	MotorAddress.PacketHead[LEFT_LEG] = &(MotorData.PacketHead_LL[0]);
	
	MotorAddress.Write[UPPER_BODY] = &(MotorData.Write_UB[0][0]);
	MotorAddress.Write[RIGHT_LEG] = &(MotorData.Write_RL[0][0]);	
	MotorAddress.Write[LEFT_LEG] = &(MotorData.Write_LL[0][0]);

  MotorAddress.Checksum[UPPER_BODY] = &(MotorData.Checksum_UB);
	MotorAddress.Checksum[RIGHT_LEG] = &(MotorData.Checksum_RL);	
	MotorAddress.Checksum[LEFT_LEG] = &(MotorData.Checksum_LL);		 
	
  MotorAddress.WtiteLength[UPPER_BODY] = &(MotorData.WtiteLength_UB);
	MotorAddress.WtiteLength[RIGHT_LEG] = &(MotorData.WtiteLength_RL);	
	MotorAddress.WtiteLength[LEFT_LEG] = &(MotorData.WtiteLength_LL);
	////////////////////////////////////////////////////////////////////////////////
 	MotorTorqueAddress.PacketHead[UPPER_BODY] = &(MotorTorque.PacketHead_UB[0]);
	MotorTorqueAddress.PacketHead[RIGHT_LEG] = &(MotorTorque.PacketHead_RL[0]);	
	MotorTorqueAddress.PacketHead[LEFT_LEG] = &(MotorTorque.PacketHead_LL[0]);
	
	MotorTorqueAddress.Write[UPPER_BODY] = &(MotorTorque.Write_UB[0][0]);
	MotorTorqueAddress.Write[RIGHT_LEG] = &(MotorTorque.Write_RL[0][0]);	
	MotorTorqueAddress.Write[LEFT_LEG] = &(MotorTorque.Write_LL[0][0]);

  MotorTorqueAddress.Checksum[UPPER_BODY] = &(MotorTorque.Checksum_UB);
	MotorTorqueAddress.Checksum[RIGHT_LEG] = &(MotorTorque.Checksum_RL);	
	MotorTorqueAddress.Checksum[LEFT_LEG] = &(MotorTorque.Checksum_LL);		 
	
  MotorTorqueAddress.WtiteLength[UPPER_BODY] = &(MotorTorque.WtiteLength_UB);
	MotorTorqueAddress.WtiteLength[RIGHT_LEG] = &(MotorTorque.WtiteLength_RL);	
	MotorTorqueAddress.WtiteLength[LEFT_LEG] = &(MotorTorque.WtiteLength_LL);
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : MotorData_Init
* Description    : Initialize MotorData 
									 The setting of PacketHead is according to the datasheet of Robotis RX-64 / RX-28 / EX-106,
									 and it is unchangeable when running process.
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void MotorData_Init(void)
{
	int i, j;
	unsigned char index_id = 0;			
	unsigned char id = 0;

	MotorAddress_Init();
	// Unchangeable	when running process
	for(i=0; i < BODYPART_NUM; i++)
	{									
		*(MotorAddress.PacketHead[i] + HEAD1) = 0xFF;
		*(MotorAddress.PacketHead[i] + HEAD2) = 0xFF;
		*(MotorAddress.PacketHead[i] + ID_SYNCWRITE) = 0xFE; // 0xFE, ID for SYNC WRITE
		*(MotorAddress.PacketHead[i] + TOTAL_LENGTH) = (1 + (MOTOR_WRITE_LENGTH-1)) * MotorNum[i] + 4;
		*(MotorAddress.PacketHead[i] + INSTRUCTION_SYNCWRITE) = 0x83; // 0x83, Instruction for SYNC WRITE 
		*(MotorAddress.PacketHead[i] + FIRST_ADDRESS) = 0x1E; // 0x1E, First address to write (low byte of goal position)
		*(MotorAddress.PacketHead[i] + SINGLE_DATA_LENGTH) = (MOTOR_WRITE_LENGTH-1);  // 0x04, length of data to write		 
		//////////////////////////////////////////////////////////////////////////////////						
		*(MotorTorqueAddress.PacketHead[i] + HEAD1) = 0xFF;
		*(MotorTorqueAddress.PacketHead[i] + HEAD2) = 0xFF;
		*(MotorTorqueAddress.PacketHead[i] + ID_SYNCWRITE) = 0xFE; // 0xFE, ID for SYNC WRITE
		*(MotorTorqueAddress.PacketHead[i] + TOTAL_LENGTH) = (1 + (MOTOR_TORQUE_LENGTH-1)) * MotorNum[i] + 4;
		*(MotorTorqueAddress.PacketHead[i] + INSTRUCTION_SYNCWRITE) = 0x83; // 0x83, Instruction for SYNC WRITE 
		*(MotorTorqueAddress.PacketHead[i] + FIRST_ADDRESS) = 0x18; // 0x18, First address to write (torque)
		*(MotorTorqueAddress.PacketHead[i] + SINGLE_DATA_LENGTH) = (MOTOR_TORQUE_LENGTH-1);  // 0x01, length of data to write

		for(j=ID_SYNCWRITE; j < PACKET_HEAD_LENGTH; j++)
		{
			MotorData.Checksum_PacketHead[i] += *(MotorAddress.PacketHead[i] + j);
			//////////////////////////////////////////////////////////////////////////////////	
			MotorTorque.Checksum_PacketHead[i] += *(MotorTorqueAddress.PacketHead[i] + j);
		}

		for(j=0; j < MotorNum[i]; j++)
		{
			if(index_id < MOTOR_NUM_UB)
			{
				id = MotorIDinUB[index_id];
			}
			else if(index_id < (MOTOR_NUM_UB + MOTOR_NUM_RL))
			{
				id = MotorIDinRL[index_id - MOTOR_NUM_UB];
			}
			else
			{												
				id = MotorIDinLL[index_id - MOTOR_NUM_UB - MOTOR_NUM_RL];
			}

			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + ID) = id;	
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_POSITION) = 0;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_POSITION) = 0x20;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + LOW_SPEED) = 0;
			*(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + HIGH_SPEED) = 0x02;
			//////////////////////////////////////////////////////////////////////////////////
			*(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + TORQUE_ID) = id;	
			*(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = 1; // Torque: ON
													
			index_id++;
		}

		*(MotorAddress.Checksum[i]) = 0;	
		//////////////////////////////////////////////////////////////////////////////////
		*(MotorTorqueAddress.Checksum[i]) = 0;		 

		// Length = #PacketHead + #Write + Checksum
		*(MotorAddress.WtiteLength[i]) = PACKET_HEAD_LENGTH + (MotorNum[i] * MOTOR_WRITE_LENGTH) + 1;		
		*(MotorTorqueAddress.WtiteLength[i]) = PACKET_HEAD_LENGTH + (MotorNum[i] * MOTOR_TORQUE_LENGTH) + 1;
	}

	// Changeable	when running process
	for(i=0; i < MOTOR_NUM; i++)
	{									 	
		if(i < MOTOR_NUM_UB)
		{
			id = MotorIDinUB[i];
		}
		else if(i < (MOTOR_NUM_UB + MOTOR_NUM_RL))
		{
			id = MotorIDinRL[i - MOTOR_NUM_UB];
		}
		else
		{												
			id = MotorIDinLL[i - MOTOR_NUM_UB - MOTOR_NUM_RL];
		}				 
		MotorData.Read[i][READ_ID] = id;	
													 		
		for(j=(READ_ID+1); j < MOTOR_READ_LENGTH; j++)
		{
			MotorData.Read[i][j] = 0;
		}
	}						

	for(i=0; i < BODYPART_NUM; i++)
	{
		MotorData.Battery[i] = 0;	
		MotorData.BatteryCount[i] = 0;
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : MotorSchedule_Init
* Description    : Initialize MotorSchedule 
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : Delay_ExecutedCount is the number that the function Update_MotorSchedule_to_MotorData idles
*******************************************************************************/
void MotorSchedule_Init(void)
{
	int i,j;

	MotorSchedule.Enabled = TRUE;
	MotorSchedule.NowIndex = 0;
 	MotorSchedule.EndIndex = 0;

	for(i=0; i < MOTOR_SCHEDULE_SIZE; i++)
	{
		for(j=0; j < MOTOR_SCHEDULE_LIST_LENGTH; j++)
		{
			MotorSchedule.List[i][j] = 0;
		}
	}
	
	MotorSchedule.Delay_ExecutedCount = 0;
	MotorSchedule.Delay_Time = 0;
	MotorSchedule.TorqueChanged = OFF;
	MotorSchedule.Capacity = EMPTY;

	MotorScheduleSwitch(MotorSchedule.Enabled);
}	 
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////		 
/*******************************************************************************
* Function Name  : HeadSchedule_Init
* Description    : Initialize HeadSchedule
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void HeadSchedule_Init(void)
{
	int i, j;
	HeadSchedule.NowIndex = 0;
 	HeadSchedule.EndIndex = 0;
	HeadSchedule.Delay_ExecutedCount = 0;
	HeadSchedule.Delay_Time = 0;

	for(i=0; i < HEAD_SCHEDULE_SIZE; i++)
	{
		for(j=0; j < HEAD_SCHEDULE_LIST_LENGTH; j++)
		{
			HeadSchedule.List[i][j] = 0;
		}
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 
/*******************************************************************************
* Function Name  : WriteMotor
* Description    : Write the values of position & speed through USART to AI-motor by DMA
* Input          : BodyPart:
										1. Upper Body: About 7.5 ms
										2. Right leg
										3. Left leg
										4. All
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void WriteMotor(BodyPartStatus part)
{	
	CalculateChecksum();

	#ifdef MOTOR_UB_PORT					
		if((part == UPPER_BODY) || (part == BODYPART_NUM))
		{									
			DIR485_Send(MOTOR_UB_PORT);			
			if(MotorWritedFinish[UPPER_BODY] == TRUE)
			{
				LedSwitch(ON, 5);			
			}				
			else
			{				
				LedSwitch(OFF, 5);	
			}					
			DMA1_Configuration(MOTOR_UB_PORT, MotorAddress.PacketHead[UPPER_BODY], *(MotorAddress.WtiteLength[UPPER_BODY]));				 		 
//			DMA2_Configuration(MotorAddress.PacketHead[UPPER_BODY], *(MotorAddress.WtiteLength[UPPER_BODY]));
			MotorWritedFinish[UPPER_BODY] = FALSE;
		}
	#endif

	#ifdef MOTOR_RL_PORT					 	
		if((part == RIGHT_LEG) || (part == BODYPART_NUM))
		{																								
			DIR485_Send(MOTOR_RL_PORT);																																				 		
			DMA1_Configuration(MOTOR_RL_PORT, MotorAddress.PacketHead[RIGHT_LEG], *(MotorAddress.WtiteLength[RIGHT_LEG]));	 			 
			MotorWritedFinish[RIGHT_LEG] = FALSE;
		}
	#endif

	#ifdef MOTOR_LL_PORT					 
		if((part == LEFT_LEG) || (part == BODYPART_NUM))
		{			 																	
			DIR485_Send(MOTOR_LL_PORT);																																		
			DMA1_Configuration(MOTOR_LL_PORT, MotorAddress.PacketHead[LEFT_LEG], *(MotorAddress.WtiteLength[LEFT_LEG]));		
			MotorWritedFinish[LEFT_LEG] = FALSE; 			
		}
	#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 
/*******************************************************************************
* Function Name  : WriteTorque
* Description    : Write the values of torque through USART to AI-motor by DMA
* Input          : BodyPart:
										1. Upper Body
										2. Right leg
										3. Left leg
										4. All
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void WriteTorque(BodyPartStatus part)
{												 
	CalculateChecksum();

	#ifdef MOTOR_UB_PORT						
		if((part == UPPER_BODY) || (part == BODYPART_NUM))
		{								 	 	
			DIR485_Send(MOTOR_UB_PORT);
			DMA1_Configuration(MOTOR_UB_PORT, MotorTorqueAddress.PacketHead[UPPER_BODY], *(MotorTorqueAddress.WtiteLength[UPPER_BODY]));	 		 
			MotorWritedFinish[UPPER_BODY] = FALSE;
		}
	#endif

	#ifdef MOTOR_RL_PORT				 
		if((part == RIGHT_LEG) || (part == BODYPART_NUM))
		{																										
			DIR485_Send(MOTOR_RL_PORT);																															 		
			DMA1_Configuration(MOTOR_RL_PORT, MotorTorqueAddress.PacketHead[RIGHT_LEG], *(MotorTorqueAddress.WtiteLength[RIGHT_LEG]));	 
			MotorWritedFinish[RIGHT_LEG] = FALSE; 			 
		}
	#endif

	#ifdef MOTOR_LL_PORT			
		if((part == LEFT_LEG) || (part == BODYPART_NUM))
		{			 																					
			DIR485_Send(MOTOR_LL_PORT);																																																
			DMA1_Configuration(MOTOR_LL_PORT, MotorTorqueAddress.PacketHead[LEFT_LEG], *(MotorTorqueAddress.WtiteLength[LEFT_LEG]));	
			MotorWritedFinish[LEFT_LEG] = FALSE; 				 			
		}
	#endif
}																	
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 
/*******************************************************************************
* Function Name  : ReadMotor
* Description    : Send the read instruction to the motor
* Input          : id
* Output         : None
* Return         : None
* Attention		 	 : ReadMotor may interupt the process of WriteMotor or WriteTorque?
*******************************************************************************/				 
Bool ReadMotor(unsigned char id_index)	
{																				
	unsigned char id = MotorData.Read[id_index][READ_ID];
	int i, temp_part, temp_no;
	int count = 0;
	int receive_num = 0;
	BodyPartStatus bodypart;
	USART_TypeDef* USARTx = NULL;	 
	unsigned char checksum = 0;	
	unsigned char ReadPacket[8] = {0xFF, 0xFF, 0x00, 0x04, 0x02, 0x24, 0x07, 0x00};
	unsigned read_byte;	
	Bool receive_finish = FALSE;
	
	MotorData.Read[id_index][READ_VOLTAGE] = 0;

	ReadPacket[2] = id;	 
	for(i=2; i < 7; i++)
	{
		checksum += ReadPacket[i];
	}											
	ReadPacket[7] = ~checksum;				 


//	if(id < MOTOR_NUM_UB)
//	{
//		bodypart = UPPER_BODY;
//		#ifdef MOTOR_UB_PORT
//		USARTx = MOTOR_UB_PORT;
//		#endif
//	}
//	else if(id < (MOTOR_NUM_UB + MOTOR_NUM_RL))
//	{													 
//		bodypart = RIGHT_LEG;
//		#ifdef MOTOR_RL_PORT
//		USARTx = MOTOR_RL_PORT;
//		#endif			
//	}
//	else if(id < MOTOR_NUM)
//	{													 
//		bodypart = LEFT_LEG;
//		#ifdef MOTOR_LL_PORT
//		USARTx = MOTOR_LL_PORT;
//		#endif			
//	}
	if(IDtoPartNo(id, &temp_part, &temp_no) == FALSE)
	{
		return FALSE;
	}

	if(temp_part == UPPER_BODY)
	{			
		bodypart = UPPER_BODY;
		#ifdef MOTOR_UB_PORT
		USARTx = MOTOR_UB_PORT;
		#endif
	}
	else if(temp_part == RIGHT_LEG)
	{											 
		bodypart = RIGHT_LEG;
		#ifdef MOTOR_RL_PORT
		USARTx = MOTOR_RL_PORT;
		#endif			
	}
	else if(temp_part == LEFT_LEG)
	{								 
		bodypart = LEFT_LEG;
		#ifdef MOTOR_LL_PORT
		USARTx = MOTOR_LL_PORT;
		#endif			
	}

	if(USARTx == NULL)
	{
		return FALSE;
	}

	MotorReadID[bodypart] = id;

	Delay(0x10);			
	DIR485_Send(USARTx);  	
	Delay(0x10);		

	for(i=0; i < 8; i++)
	{
		USART_SendData(USARTx, ReadPacket[i]);
		while(USART_GetFlagStatus(USARTx, USART_FLAG_TC) != SET)
		{
		}						
	}			 	
	Delay(10);			
	DIR485_Receive(USARTx);			 
	Delay(10);
	
	while(!receive_finish)
	{
		if(USART_GetFlagStatus(USARTx, USART_FLAG_RXNE) != RESET)
		{
			read_byte = USART_ReceiveData(USARTx);				

			MotorRX[bodypart].Buff[(MotorRX[bodypart].EndIndex)] = read_byte;
			MotorRX[bodypart].EndIndex = (MotorRX[bodypart].EndIndex + 1) % MOTOR_RX_BUFF_SIZE;
			receive_num++;	 
																							
			if(receive_num >= 13)	
			{
				receive_num = 0;			 	
				receive_finish = TRUE;
				return CheckReceive(bodypart);
			}
		}
		count++;
		if(count >= 3000)
		{
			break;
		}
	}
									 
	return FALSE;												
}		
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 
/*******************************************************************************
* Function Name  : CheckReceive
* Description    : Check the packet returned from the motor
* Input          : None
* Output         : None
* Return         : Succeed or not
* Attention		 	 : ReadMotor may interupt the process of WriteMotor or WriteTorque?
*******************************************************************************/				 
Bool CheckReceive(BodyPartStatus bodypart)
{							       
	int id, id_index;
	unsigned char byte_read[13]; // 13 is the length of the returned packet from the motor
															 // This number shouldn't be changed arbitrarily.
	int i = 0;
	Bool read_succeed = FALSE;

	id = MotorReadID[bodypart];
	id_index = ID2Index(id);
	
	while(MotorRX[bodypart].NowIndex != MotorRX[bodypart].EndIndex)
	{
		for(i=0; i < 13; i++)
		{
			byte_read[i] = MotorRX[bodypart].Buff[(MotorRX[bodypart].NowIndex + i) % MOTOR_RX_BUFF_SIZE];	
			
		}
//		if((byte_read[0] == 255) && (byte_read[1] == 255) && (byte_read[2] == id) &&
//			 (byte_read[3] == 9) && (byte_read[4] == 0))		
		if((byte_read[0] == 255) && (byte_read[1] == 255) && (byte_read[2] == id) &&
			 (byte_read[3] == 9) && (id_index != -1))
		{					
			MotorData.Read[id_index][READ_LOW_POSITION] = byte_read[5];	
			MotorData.Read[id_index][READ_HIGH_POSITION] = byte_read[6];
			MotorData.Read[id_index][READ_LOW_LOADING] = byte_read[9];	
			MotorData.Read[id_index][READ_HIGH_LOADING] = byte_read[10];
			MotorData.Read[id_index][READ_VOLTAGE] = byte_read[11];	 

			MotorRX[bodypart].NowIndex = (MotorRX[bodypart].NowIndex + 13) % MOTOR_RX_BUFF_SIZE;
			MotorRX[bodypart].NowIndex = 0;
			MotorRX[bodypart].EndIndex = 0;		
			read_succeed = TRUE;			
			break;
		}
		else
		{
			MotorRX[bodypart].NowIndex = (MotorRX[bodypart].NowIndex + 1) % MOTOR_RX_BUFF_SIZE;
		}
	}

	return read_succeed;
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	
/*******************************************************************************
* Function Name  : CalculateChecksum
* Description    : Calculate checksum of MotorData.Write
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : Must be done before WriteMotor 
*******************************************************************************/
void CalculateChecksum(void)
{
	unsigned char i, j, k;

	
	for(i=0; i < BODYPART_NUM; i++)
	{				 
		*(MotorAddress.Checksum[i]) = MotorData.Checksum_PacketHead[i];		
		*(MotorTorqueAddress.Checksum[i]) = MotorTorque.Checksum_PacketHead[i];

		for(j=0; j < MotorNum[i]; j++)
		{
			for(k=0; k < MOTOR_WRITE_LENGTH; k++)
			{
				*(MotorAddress.Checksum[i]) = *(MotorAddress.Checksum[i]) + *(MotorAddress.Write[i] + (j*MOTOR_WRITE_LENGTH) + k);
			}
			for(k=0; k < MOTOR_TORQUE_LENGTH; k++)
			{
				*(MotorTorqueAddress.Checksum[i]) = *(MotorTorqueAddress.Checksum[i]) + *(MotorTorqueAddress.Write[i] + (j*MOTOR_TORQUE_LENGTH) + k);
			}
		}

		*(MotorAddress.Checksum[i]) = ~(*(MotorAddress.Checksum[i]));		
		*(MotorTorqueAddress.Checksum[i]) = ~(*(MotorTorqueAddress.Checksum[i]));
	}
																										 	
}																										///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	
/*******************************************************************************
* Function Name  : CalculateReadChecksum
* Description    : Calculate checksum of MotorData.Read
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : Must be done before WriteMotor 
*******************************************************************************/
void CalculateReadChecksum(void)
{
	int i, j;
	
	for(i=0; i < MOTOR_NUM; i++)
	{				
		MotorData.Read[i][READ_CHECKSUM] = 0; 

		for(j=0; j < READ_CHECKSUM; j++)
		{
			MotorData.Read[i][READ_CHECKSUM] += MotorData.Read[i][j]; 
		}

		MotorData.Read[i][READ_CHECKSUM] = ~MotorData.Read[i][READ_CHECKSUM];
	}																																			 																										 	
}																										
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 	
/*******************************************************************************
* Function Name  : MotorScheduleSwitch
* Description    : Turn on or turn off the MotorSchedule
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/																						
void MotorScheduleSwitch(WorkStatus status)
{
	if(status == ON)
	{
		MotorSchedule.Enabled = TRUE;
		#ifdef SCHEDULE_UPDATE_TIMER	
			TIM_Cmd(SCHEDULE_UPDATE_TIMER, ENABLE);						
		#endif
	}
	else
	{	 
		MotorSchedule.Enabled = FALSE;
		#ifdef SCHEDULE_UPDATE_TIMER	
			TIM_Cmd(SCHEDULE_UPDATE_TIMER, DISABLE);
		#endif
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	 	
/*******************************************************************************
* Function Name  : Update_MotorSchedule_to_MotorData
* Description    : Update MotorSchedule to MotorData. Then, call WriteMotor if necessary.
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : MOTOR_WRITE_TIMER should be disabled at first, and be enabled in the end.
*******************************************************************************/
void Update_MotorSchedule_to_MotorData(void)
{
	int i;
	unsigned char motor_ID;
	int Part, No;	 

	MotorBeWrited = FALSE;

	// Decide what to do
	if(MotorSchedule.NowIndex != MotorSchedule.EndIndex)
	{								 		
		if(MotorSchedule.Delay_ExecutedCount > 0)
		{
			MotorSchedule.Delay_ExecutedCount--;
			if(MotorSchedule.Delay_ExecutedCount == 0)
			{
				MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE ;
			}
		} 
		else
		{
			// Update MotorData.Write, Torque or steptime, until INSTRUCTION_MODE is InsertDelayCount
			while((MotorSchedule.NowIndex != MotorSchedule.EndIndex) && 
						(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] != INSERT_DELAY_COUNT))
			{
				if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] <= MOTOR_MAX_ID)
				{
					// if INSTRUCTION_MODE < SetPosition, then INSTRUCTION_MODE = motor ID
					// Therefore, MotorSchedule.List[MotorShedule.NowIndex] = {ID, LowPosion, HighPosition, LowSpeed, HighSpeed}
					motor_ID = MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE];						
					IDtoPartNo(motor_ID, &Part, &No);
					
					if(IDtoPartNo(motor_ID, &Part, &No) == TRUE) 
					{
							if((*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + ID)) == motor_ID)
							{
								// Add correction value to motor data, if necessary
								if(MotorData_Correction[ID2Index(motor_ID)] == 0)
								{
									for(i=LOW_POSITION; i < MOTOR_WRITE_LENGTH; i++)
									{
										*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + i) = MotorSchedule.List[MotorSchedule.NowIndex][i];
									}
								}
								else
								{
									int temp = MotorSchedule.List[MotorSchedule.NowIndex][1] | (MotorSchedule.List[MotorSchedule.NowIndex][2] << 8);
									temp = temp + MotorData_Correction[ID2Index(motor_ID)];
			
									*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + LOW_POSITION) = temp & 0x00FF;
									*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + HIGH_POSITION) = (temp & 0xFF00) >> 8;
									*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + LOW_SPEED) = MotorSchedule.List[MotorSchedule.NowIndex][3];
									*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + HIGH_SPEED) = MotorSchedule.List[MotorSchedule.NowIndex][4];
			
								}
								// Set torque
							  *(MotorTorqueAddress.Write[Part] + (No * MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = 1;
								// Motor data are updated completely
								MotorBeWrited = TRUE;
							} // End if: motor_ID recheck

					}
					else
					{
					}
					
				}	// End if: SetPosition
				else if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] < SET_TORQUE)
				{
					// if INSTRUCTION_MODE < SetTorque, then INSTRUCTION_MODE-TORQUE_BASE = motor ID
					motor_ID = MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] - TORQUE_BASE;													
					IDtoPartNo(motor_ID, &Part, &No);

					if((*(MotorTorqueAddress.Write[Part] + (No * MOTOR_TORQUE_LENGTH) + TORQUE_ID)) == motor_ID)
					{
					  *(MotorTorqueAddress.Write[Part] + (No * MOTOR_TORQUE_LENGTH) + TORQUE_ENABLED) = MotorSchedule.List[MotorSchedule.NowIndex][1];
						MotorSchedule.TorqueChanged = ON;
					}
				} 
				else if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] == SET_STEP_TIME)
				{
					unsigned int new_prescaler;
					Steptime_WriteMotor =  MotorSchedule.List[MotorSchedule.NowIndex][1] * 10;
					new_prescaler = (((float)Steptime_WriteMotor) / 1000) * 8000;

					new_prescaler = (new_prescaler < 600) ? 600 : (new_prescaler > 3000) ? 3000 : new_prescaler;
					TIM_PrescalerConfig(SCHEDULE_UPDATE_TIMER, new_prescaler, TIM_PSCReloadMode_Update);
				} 

				MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE;
			} // End while
 
			if(MotorSchedule.NowIndex != MotorSchedule.EndIndex)
			{	// This if statement is replenished on 2012/07/30 to prevent out-of-order index.
				if(MotorSchedule.List[MotorSchedule.NowIndex][INSTRUCTION_MODE] == INSERT_DELAY_COUNT)
				{
					MotorSchedule.Delay_ExecutedCount = MotorSchedule.List[MotorSchedule.NowIndex][1] - 1;
					if(MotorSchedule.Delay_ExecutedCount <= 0)
					{
						 MotorSchedule.NowIndex = (MotorSchedule.NowIndex + 1) % MOTOR_SCHEDULE_SIZE;
					}
				}
			}	// End if: NowIndex != EndIndex
		} // End if: Delay = 0 
	}	// End if: Updata schedule (Decide what to do)


	if((MotorSchedule.Delay_ExecutedCount == 0) && (MotorSchedule.NowIndex == MotorSchedule.EndIndex))
	{
		MotorSchedule.NowIndex = 0;
		MotorSchedule.EndIndex = 0;
		MotorSchedule.Capacity = EMPTY;

		#ifdef DEBUG_PORT
		if(Zigbee_Disabled == FALSE)
		{
			USART_ITConfig(DEBUG_PORT, USART_IT_RXNE, ENABLE);
			Zigbee_Ready2Disable = FALSE;
		}
		#endif
	}
	if(MotorSchedule.NowIndex == MotorSchedule.EndIndex)
	{
		if(Motion.Continue_Eanble == TRUE || Motion.Mode > 100) {
      Motion.Doing = Motion.Continue_Set;
    }
    else if(MotorSchedule.Delay_ExecutedCount == 0) {
      Motion.Doing = 0;
    }
	}

	// Change the torque
  if(MotorSchedule.TorqueChanged == ON) {	
    WriteTorque(BODYPART_NUM);
    MotorSchedule.TorqueChanged = OFF;
  }

	if(MotorBeWrited == TRUE)
	{
//		// Write all
		WriteMotor(BODYPART_NUM);	
	}

}										
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : Update_HeadSchedule_to_MotorData
* Description    : Be Similary as Update_MotorSchedule_to_MotorData, but only be used by the command of COMP or HMI.
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : MOTOR_WRITE_TIMER should be disabled at first, and be enabled in the end.
*******************************************************************************/
void Update_HeadSchedule_to_MotorData(void)
{
	int i;
	unsigned char motor_ID;
	int Part, No;							 
	HeadBeWrited = FALSE;

//	#ifdef MOTOR_WRITE_TIMER
//		TIM_Cmd(MOTOR_WRITE_TIMER, DISABLE);
//	#endif		 

	// Decide what to do 		
	if(HeadSchedule.NowIndex != HeadSchedule.EndIndex)
	{
		if(HeadSchedule.Delay_ExecutedCount > 0)
		{
			HeadSchedule.Delay_ExecutedCount--;
			if(HeadSchedule.Delay_ExecutedCount == 0)
			{
				HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
			}
		} 
		else
		{
			// Update MotorData.Write, Torque or steptime, until INSTRUCTION_MODE is InsertDelayCount
			while((HeadSchedule.NowIndex != HeadSchedule.EndIndex) && 
						(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] != INSERT_DELAY_COUNT))
			{
				if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] < MOTOR_MAX_ID)
				{
					// if INSTRUCTION_MODE < SetPosition, then INSTRUCTION_MODE = motor ID
					// Therefore, HeadSchedule.List[HeadShedule.NowIndex] = {ID, LowPosion, HighPosition, LowSpeed, HighSpeed}
					motor_ID = HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE];						
					IDtoPartNo(motor_ID, &Part, &No);
									
					if((*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + ID)) == motor_ID)
					{
						// Add correction value to motor data, if necessary
						if(MotorData_Correction[ID2Index(motor_ID)] == 0)
						{
							for(i=LOW_POSITION; i < MOTOR_WRITE_LENGTH; i++)
							{
								*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + i) = HeadSchedule.List[HeadSchedule.NowIndex][i];
							}
						}
						else
						{
							int temp = HeadSchedule.List[HeadSchedule.NowIndex][1] | (HeadSchedule.List[HeadSchedule.NowIndex][2] << 8);
							temp = temp + MotorData_Correction[ID2Index(motor_ID)];
		
							*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + LOW_POSITION) = temp & 0x00FF;
							*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + HIGH_POSITION) = (temp & 0xFF00) >> 8;
							*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + LOW_SPEED) = HeadSchedule.List[HeadSchedule.NowIndex][3];
							*(MotorAddress.Write[Part] + (No * MOTOR_WRITE_LENGTH) + HIGH_SPEED) = HeadSchedule.List[HeadSchedule.NowIndex][4];
						}
						// Head data are updated completely
						HeadBeWrited = TRUE;
					} // End if: motor_ID recheck
				}	// End if: SetPosition
				else if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] == SET_STEP_TIME)
				{
					Steptime_WriteHead =  HeadSchedule.List[HeadSchedule.NowIndex][1] * 10;
				} 

				HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
			} // End while
 
			if(HeadSchedule.List[HeadSchedule.NowIndex][INSTRUCTION_MODE] == INSERT_DELAY_COUNT)
			{
				HeadSchedule.Delay_ExecutedCount = HeadSchedule.List[HeadSchedule.NowIndex][1] - 1;
				if(HeadSchedule.Delay_ExecutedCount <= 0)
				{
					 HeadSchedule.NowIndex = (HeadSchedule.NowIndex + 1) % HEAD_SCHEDULE_SIZE;
				}
			}
		} // End if: Delay = 0 
	}	// End if: Updata schedule (Decide what to do)


	if((HeadSchedule.Delay_ExecutedCount == 0) && (HeadSchedule.NowIndex == HeadSchedule.EndIndex))
	{
		HeadSchedule.NowIndex = 0;
		HeadSchedule.EndIndex = 0;
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////// 	
/*******************************************************************************
* Function Name  : Insert_MotorSchedule
* Description    : None
* Input          : data0 ~ data4 (usually: ID, LP, HP, LS, HS)
* Output         : None
* Return         : Successed / Fail
* Attention		 	 : None
*******************************************************************************/			 
Bool Add_motor_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4)
{												 
//	if(MotorSchedule.Capacity != FULL)
//	{
		MotorSchedule.List[MotorSchedule.EndIndex][0] = data0;
	  MotorSchedule.List[MotorSchedule.EndIndex][1] = data1;
	  MotorSchedule.List[MotorSchedule.EndIndex][2] = data2;
	  MotorSchedule.List[MotorSchedule.EndIndex][3] = data3;
	  MotorSchedule.List[MotorSchedule.EndIndex][4] = data4;
	  MotorSchedule.EndIndex = (MotorSchedule.EndIndex + 1) % MOTOR_SCHEDULE_SIZE;
	
		if((MotorSchedule.NowIndex == 0) && (MotorSchedule.EndIndex == (MOTOR_SCHEDULE_SIZE-1)))
		{
			MotorSchedule.Capacity = FULL;
		}
		else if((MotorSchedule.EndIndex - MotorSchedule.NowIndex) == 1)
		{
			MotorSchedule.Capacity = FULL;
		}
		else
		{
			MotorSchedule.Capacity = NONEMPTY;
		}

		return TRUE;
//	}
//	else
//	{
//		return FALSE;
//	}
}					
///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////// 
/*******************************************************************************
* Function Name  : Insert_HeadSchedule
* Description    : None
* Input          : data0 ~ data4 (usually: ID, LP, HP, LS, HS)
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/			 
void Add_head_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4)
{
 	HeadSchedule.List[HeadSchedule.EndIndex][0] = data0;
  HeadSchedule.List[HeadSchedule.EndIndex][1] = data1;
  HeadSchedule.List[HeadSchedule.EndIndex][2] = data2;
  HeadSchedule.List[HeadSchedule.EndIndex][3] = data3;
  HeadSchedule.List[HeadSchedule.EndIndex][4] = data4;						
				
  HeadSchedule.EndIndex = (HeadSchedule.EndIndex + 1) % HEAD_SCHEDULE_SIZE;
}		 
///////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////// 
/*******************************************************************************
* Function Name  : IDtoPartNo
* Description    : None
* Input          : id
* Output         : part & no
* Return         : None
* Attention		 	 : For MotorData.Write
*******************************************************************************/			 
Bool IDtoPartNo(unsigned char id, int *part, int *no)
{
	unsigned char i = 0;
	if((id == 0) || (id == 1) || (id == 2) || (id == 3) || (id == 4) ||
	   (id == 10) || (id == 11)|| (id == 12) || (id == 13)||(id == 14) || 
		 (id == 20) || (id == 21)|| (id == 22) || (id == 23)||(id == 24) ||
		 (id == 30) || (id == 31)|| (id == 32) || (id == 33)||(id == 34) || (id == 35) ||
		 (id == 40) || (id == 41)|| (id == 42) || (id == 43)||(id == 44) || (id == 45))
	{
		for(i=0; i < MOTOR_NUM_UB; i++)
		{
			if(MotorIDinUB[i] == id)
			{
				*part = UPPER_BODY;	 
				*no = i;	
				return TRUE;			
			}
		}

		for(i=0; i < MOTOR_NUM_RL; i++)
		{
			if(MotorIDinRL[i] == id)
			{
				*part = RIGHT_LEG;	 
				*no = i;	
				return TRUE;			
			}
		}			 

		for(i=0; i < MOTOR_NUM_LL; i++)
		{
			if(MotorIDinLL[i] == id)
			{
				*part = LEFT_LEG;	 
				*no = i;	
				return TRUE;			
			}
		}


	}
			
	*part = 0;
	*no = 0;
	return FALSE;
}

/*******************************************************************************
* Function Name  : ID2CorrectionIndex
* Description    : None
* Input          : id
* Output         : correction motor index
* Return         : None
* Attention		 	 : For MotorData.Write
*******************************************************************************/
int ID2Index(unsigned char id)
{
	int i=0;
	for(i=0; i < MOTOR_NUM_UB; i++)
	{
		if(MotorIDinUB[i] == id)
		{
			return i;
		}
	}
	for(i=0; i < MOTOR_NUM_RL; i++)
	{
		if(MotorIDinRL[i] == id)
		{
			return (i+MOTOR_NUM_UB);
		}
	}
	for(i=0; i < MOTOR_NUM_LL; i++)
	{
		if(MotorIDinLL[i] == id)
		{
			return (i+MOTOR_NUM_UB+MOTOR_NUM_RL);
		}
	}
	return -1;
}